#include "Tetra4ThermalCalculator.h"
#include "Utility/Algebra/Tools.h"
#include "DataStructure/Input/Load/SLOADData.h"
#include "DBManager/IService/Input/Node/INodeService.h"
#include "DBManager/IService/Input/Element/IElementService.h"
#include "DBManager/IService/Input/Subcase/ISubcaseService.h"
#include "DBManager/IService/Input/Property/IPSOLIDService.h"
#include "DBManager/IService/Input/Material/IThermalMaterialService.h"

#include "SGFem/Pre/Assembly/Assembly.h"
#include "SGFem/Pre/Assembly/ModelInfoQuery.h"
// #include "../Common/ElementTools.hpp"

using SG::Algebra::Point;
using SG::Algebra::Vector3D;
using SG::Algebra::Matrix;
using SG::Algebra::identity;
using SG::Algebra::det3X3;
using SG::Algebra::calcTriArea;
using SG::Algebra::calcTetraVolumn;
using SG::DataStructure::Common::Id;
using SG::DataStructure::Common::Real;
// using SG::DataStructure::Common::TOLERANCE;
using SG::DataStructure::FEM::ThermalMaterial;
using SG::DataStructure::FEM::PSOLIDData;
using namespace SG::DataStructure::FEM;
using namespace SG::DBManager;
using SG::FEMSolver::AssemblyEigen;
using SG::FEMSolver::Element::Tetra4ThermalCalculator;
std::size_t Tetra4ThermalCalculator::s_nNode = 4;
std::size_t Tetra4ThermalCalculator::s_nDim = 3;


void Tetra4ThermalCalculator::Initialize (SG::DataStructure::FEM::ElementTypes type)
{
    m_type = type;
    
}

void Tetra4ThermalCalculator::ComputeStrainMatrix(const Matrix& vertices, const Real &volumn, _OUT Matrix& B) const
{
    Matrix ones(4, 1, 1.0);
    std::vector<Real> beta(s_nNode);
    std::vector<Real> gama(s_nNode);
    std::vector<Real> delta(s_nNode);

    auto tmpV = hstack(ones, vertices);
    for (std::size_t i = 0; i < 4; ++i)
    {
        beta[i] = std::pow(-1, i+1) * det3X3(remove(remove(tmpV, i, 0), 1, 1));
        gama[i] = std::pow(-1, i+2) * det3X3(remove(remove(tmpV, i, 0), 2, 1));
        delta[i] = std::pow(-1, i+1) * det3X3(remove(remove(tmpV, i, 0), 3, 1));

        B(0, i) = beta[i] / (6.0 * volumn);
        B(1, i) = gama[i] / (6.0 * volumn);
        B(2, i) = delta[i] / (6.0 * volumn);
    }

}

void Tetra4ThermalCalculator::ComputeStiffness
(
    const Matrix& vertices,    
    const ThermalMaterial& material,
    _OUT Matrix& ke
)
{
    Matrix D(s_nDim, s_nDim);
    Matrix B(s_nDim, s_nNode);
    //Matrix ke(3, 3);
    Real volumn;
    
    const Real& k   = material.m_K;
    const Real& kx = k;
    const Real& ky = k;
    const Real& kz = k;
    D(0, 0) = kx;
    D(1, 1) = ky;
    D(2, 2) = kz;
    
    volumn = calcTetraVolumn(vertices);
    ComputeStrainMatrix (vertices, volumn, _OUT B);
    auto BT = B.Transpose ();
    ke = BT * D * B * volumn;
}

void Tetra4ThermalCalculator::ComputeCapacityStiffness
    (
    const SG::Algebra::Matrix& vertices, 
    const SG::DataStructure::FEM::ThermalMaterial& material,
    _OUT SG::Algebra::Matrix& ke3
    )
{
    Real volumn;
    Real m_rho = material.m_RHO;
    Real m_c = material.m_CP;
    volumn = calcTetraVolumn (vertices);

    ke3 = Matrix (4, 4, {2.0, 1.0, 1.0, 1.0, 1.0, 2.0, 1.0, 1.0, 1.0, 1.0, 2.0, 1.0, 1.0, 1.0, 1.0, 2.0});
    ke3 = ke3 * (m_rho * m_c * volumn / 20.0);

}

void Tetra4ThermalCalculator::ComputeHeatSourceLoad
    (
    const SG::Algebra::Matrix& coords,
    const std::vector<Real>& q,
    _OUT SG::Algebra::Matrix& p1
    )
{
    Real volumn;
    volumn = calcTetraVolumn (coords);
    
    Real qsum = (q[0] + q[1] + q[2] + q[3]) / 4.0;
    p1 = Matrix (4, 1, {1.0, 1.0, 1.0, 1.0});
    p1 = p1 * (qsum * volumn / 4.0);

}

void Tetra4ThermalCalculator::ComputeElementMatries (const SG::DBManager::DBServiceFactorySP dbServiceFactory)
{
    auto& assembly = AssemblyEigen::GetInstance ();
    
    static const DofSet DOFs {DOFDirections::DOF_Ux};

    SG::DataStructure::FEM::CTETRA4Data eleData;
    PSOLIDData property;   
    ThermalMaterial material;
    
    std::vector<std::size_t> nodeIndexes(s_nNode);
    std::vector<Id> dispCoordIds(s_nNode);
    std::vector<int> dofGlobalIndex (s_nNode);
    std::vector<Point> points(s_nNode);

    //获取节点热源
    // IdArray           sidSet;
    std::vector<Real> heatSource;
    //单元工具里面需要有一个函数能获取
    //ElementTools::ComputeHeatSourceVectors (dbServiceFactory, _OUT sidSet, _OUT heatSource);
    // const auto   nGrav = sidSet.size ();
    
    const auto& tetra4Service = dbServiceFactory ->get<IElementService>();
    const auto& nodeService = dbServiceFactory ->get<INodeService>();
    const auto eleSize = tetra4Service->GetElementSize (m_type);
    for (std::size_t index{0}; index < eleSize; ++index)
    {
        /** 从数据库获取对应的单元数据 */
        tetra4Service->FindByIndex (index, _OUT eleData);

        // Node coordinate
        ModelInfoQuery::QueryNode (nodeService, eleData.m_g, s_nNode, _OUT points, _OUT nodeIndexes, _OUT dispCoordIds);
        assembly.GetGlobalDOFId (nodeIndexes, DOFs, _OUT dofGlobalIndex);
        Matrix vertices(s_nNode, s_nDim);
        // assembly.FindNodeIndex (eleData.m_g, _OUT nodeIndexes);
        for (std::size_t iNode = 0; iNode < s_nNode; ++iNode)
        {
            vertices(iNode, 0) = points[iNode].m_x;
            vertices(iNode, 1) = points[iNode].m_y;
            vertices(iNode, 2) = points[iNode].m_z;
        }
         
        //获取材料参数
        dbServiceFactory ->get<IPSOLIDService>()->FindById (eleData.m_pId, _OUT property);
        dbServiceFactory ->get<IThermalMaterialService>()->FindById (property.m_mId, _OUT material);
        
        Matrix ke1(s_nNode, s_nNode);
        ComputeStiffness (vertices, material, _OUT ke1);
        assembly.AccumulateStiffness (dofGlobalIndex, ke1);
    }
}

// 后处理数据
void Tetra4ThermalCalculator::DataRecovery (const SG::DBManager::DBServiceFactorySP dbServiceFactory, int subcaseId, SG::DataStructure::Common::Real time, const std::vector<SG::DataStructure::Common::Real>& globalDisplacement)
{
}

// 计算全局坐标系下单元应力刚度矩阵
void Tetra4ThermalCalculator::ComputeStressStiffness (const SG::DBManager::DBServiceFactorySP dbServiceFactory, int staticSubId)
{
}
